// Include files.
#include "RobotSubsystem.hpp"

/**
 *  Constructor for RobotSubsystem.
 */
RobotSubsystem::RobotSubsystem()
{
}

/**
 *  Destructor for RobotSubsystem.
 */
RobotSubsystem::~RobotSubsystem()
{
}

/**
 *  Stops the subsystem, which should stop sending power to contained actuators until an Action function is called.
 */
void RobotSubsystem::Stop()
{
	Action action(this);
	
	HandleStop();
}

/**
 *  Constructor for Action.
 */
RobotSubsystem::Action::Action(const RobotSubsystem *subsystem, bool takeSolenoid)
{
	if (takeSolenoid)
		m_semaphorePtr = &(subsystem->m_semaphore);
	else
		m_semaphorePtr = NULL;
	
	m_semaphorePtr->take();
}

/**
 *  Destructor for Action.
 */
RobotSubsystem::Action::~Action()
{
	if (m_semaphorePtr)
		m_semaphorePtr->give();
}

/**
 *  This function should be overridden to handle periodic subsystem tasks.
 *  It will be called periodically when subsystem is added to robot.
 */
void RobotSubsystem::HandlePeriodic()
{
}

bool RobotSubsystem::GetIsAwake() const
{
	return GetIsSubscribed();
}

/**
 *  This function should be overridden to handle the subsystem awakening (when Awake function is called).
 */
void RobotSubsystem::HandleAwake()
{
}

/**
 *  Awake the subsystem to reenable the periodic loop handler.
 */
void RobotSubsystem::Awake()
{
	HandleAwake();
	SetIsSubscribed(true);
}

/**
 *  Stop the subsystem and set it asleep to disable the periodic loop handler.
 */
void RobotSubsystem::Sleep()
{
	SetIsSubscribed(false);
}
